#include <memory.h>
#include "hrd_solver.h"
#include "hrd_bitmask.h"
#include "hrd_logger.h"
#include "circle_queue.h"
#include "hash_table.h"


typedef struct situ_node_s {
    hrd_path_t path;
    hrd_bitmask_list_t situation;
} situ_node_t;


static inline void appendAction(hrd_action_t *action, hrd_path_t *hrd_path)
{
    uint8_t current_length = hrd_path->path_length;

    hrd_path->path[current_length] = *action;
    hrd_path->path_length = current_length + 1;
}


static inline hrd_object_move_direction_et getRevertDirection(hrd_object_move_direction_et direction)
{
    switch (direction) {
        case MOVE_UP:
            return MOVE_DOWN;
        case MOVE_DOWN:
            return MOVE_UP;
        case MOVE_LEFT:
            return MOVE_RIGHT;
        case MOVE_RIGHT:
            return MOVE_LEFT;
    }
}


static inline bool isRevertingFather(hrd_action_t *action, situ_node_t *father_node)
{
    hrd_action_t *father_last_action = &(father_node->path.path[father_node->path.path_length - 1]);

    LOG_DETAIL("father %d: %d %d", father_last_action->object_index, father_last_action->direction, father_last_action->length);

    return action->object_index == father_last_action->object_index &&
           action->direction == getRevertDirection(father_last_action->direction) &&
           action->length == father_last_action->length;

}


static situ_node_t *enqueueSituationsObjectCanMakes(uint8_t object_index, hrd_object_allowance_t *object_allowance, situ_node_t *father_node, circle_queue_t *cir_queue)
{
    uint8_t i;
    situ_node_t new_node;
    hrd_action_t action = {.object_index = object_index};
    uint32_t queue_size = cir_queue->size;

    for (i = 0; i < object_allowance->allowed_directions_number; i++) {
        action.direction = object_allowance->allowed_directions[i];

        for (
            action.length = 1;
            HrdBitmask_CanUpdateObject(father_node->situation, action.object_index, action.direction, action.length);
            action.length++
        ) {
            if (isRevertingFather(&action, father_node)) {    /* this case can be optimized, since only one object will revert father */
                continue;
            }

            memcpy(&new_node, father_node, sizeof(situ_node_t));
            appendAction(&action, &new_node.path);
            LOG_DETAIL("path length = %d", new_node.path.path_length);

            HrdBitmask_UpdateObject(new_node.situation, action.object_index, action.direction, action.length);
            if (!CircleQueue_Enqueue(cir_queue, &new_node)) {
                LOG_FATAL("Cannot enqueue!");
                return;
            }
            if (queue_size != cir_queue->size) {
                father_node = CircleQueue_GetUpdatedHead(cir_queue);
            }

            LOG_DETAIL("Enqueued %d: %d %d", action.object_index, action.direction, action.length);
        }
    }

    return father_node;
}


static uint16_t hash(void *situation)
{
#define INV_BOUNDARY_MASK (~(DEFAULT_BOUNDARY_BITMASK))
    uint64_t hash_value = *(uint64_t *)situation;

    hash_value &= INV_BOUNDARY_MASK;
    hash_value ^= (hash_value >> 16) ^ (hash_value >> 32);

    return hash_value >> 8;
#undef INV_BOUNDARY_MASK
}


static bool solve(hrd_problem_t *problem, circle_queue_t *cir_queue, hrd_path_t *solution)
{
    uint8_t i;
    situ_node_t *node = NULL;
    hash_table_t *hash_table = NULL;

    hash_table = HashTable_NewTable(sizeof(hrd_bitmask_list_t), hash);
    if (hash_table == NULL) {
        LOG_FATAL("Can not new hash table.");
        return false;
    }

    node = CircleQueue_Dequeue(cir_queue);
    if (node == NULL) {
        HashTable_DestroyTable(hash_table);
        LOG_FATAL("No node initially.");
        return false;
    }

    while (node != NULL) {
        LOG_DETAIL("Working on situation %lld", node->situation[0]);
        if (HrdBitmask_IsGoalAchieved(problem->goal, node->situation)) {
            break;
        }
        if (node->path.path_length == MAX_PATH_LENGTH) {
            node = NULL;
            break;
        }
        if (HashTable_IsValueInTable(hash_table, node->situation)) {
            LOG_DETAIL("Accessed before, skip");
            node = CircleQueue_Dequeue(cir_queue);
            LOG_DETAIL("Dequeue %p", node);
            continue;
        }

        if (!HashTable_InsertValueToTable(hash_table, node->situation)) {
            LOG_WARNING("Failed to insert situation.");
        }
        for (i = 1; i <= problem->object_number; i++) {
            node = enqueueSituationsObjectCanMakes(i, &(problem->object_allowance_list[i]), node, cir_queue);
        }

        node = CircleQueue_Dequeue(cir_queue);
        LOG_DETAIL("Dequeue %p", node);
    }

    if (node != NULL) {
        *solution = node->path;
    }

    HashTable_DestroyTable(hash_table);

    return true;
}


hrd_path_t HrdSolver_Solve(hrd_problem_t *problem)
{
    hrd_path_t solution_path = {
        .path_length = 0,
        .path = {0}
    };
    situ_node_t initial_node;
    hrd_action_t initial_action = {.object_index = MAP_BITMASK_INDEX};
    circle_queue_t *cir_queue = CircleQueue_NewQueue(sizeof(situ_node_t));
    if (cir_queue == NULL) {
        LOG_FATAL("No memory for queue.");
        return solution_path;
    }

    memset(&initial_node, 0, sizeof(situ_node_t));
    appendAction(&initial_action, &initial_node.path);
    memcpy(initial_node.situation, problem->bitmask_list, sizeof(hrd_bitmask_list_t));
    CircleQueue_Enqueue(cir_queue, &initial_node);

    if (solve(problem, cir_queue, &solution_path) == false) {
        LOG_FATAL("Error occur while solving.");
    }
    CircleQueue_Destroy(cir_queue);

    return solution_path;
}